/*******************************************************************************
                      閿熸枻鎷锋潈閿熸枻鎷烽敓鏂ゆ嫹 (C), NCUROBOT
 *******************************************************************************
  閿熸枻鎷� 閿熸枻鎷� 閿熸枻鎷�   : V2.0
  閿熸枻鎷�    閿熸枻鎷�   : 閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�
  閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹   : 2021.1.27
  閿熸枻鎷烽敓鏂ゆ嫹钖烽敓锟�   : 2024.3.7
  閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹   : 
  閿熸枻鎷烽敓鏂ゆ嫹閿熷彨鎲嬫嫹   : 
*******************************************************************************/
/* 閿熸枻鎷烽敓鏂ゆ嫹澶撮敓渚ョ》鎷� ----------------------------------------------------------------*/
#include "can_controller.h"
#include "imu.h"
#include "pid.h"
#include "vision.h"
#include "gimbal_use.h"

/* 閿熻妭璇ф嫹閿熺枼瀹氶敓鏂ゆ嫹 ----------------------------------------------------------------*/



Gimbal_Status_t gimbal_status={REMOTE_MODE,0};
uint8_t Identification_status;

/* 閿熻妭璇ф嫹閿熺殕璁规嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熼叺鐨勬唻鎷烽敓鏂ゆ嫹 --------------------------------------------------*/
PID_TypeDef pitch_startPIDspeed          ={0};
PID_TypeDef pitch_startPIDpos            ={0};
//閿熸枻鎷烽敓閾版唻鎷烽敓鏂ゆ嫹閿熸枻鎷锋ā寮忎娇閿熸枻鎷�
PID_TypeDef yaw_rc_motor_PIDspeed 	 	  	= {0};
PID_TypeDef yaw_rc_motor_PIDpos 		  	= {0};
PID_TypeDef pitch_rc_motor_PIDspeed 		= {0};
PID_TypeDef pitch_rc_motor_PIDpos 	 		= {0};
//閿熸枻鎷烽敓鏂ゆ嫹宸￠敓鏂ゆ嫹妯″紡浣块敓鏂ゆ嫹
PID_TypeDef yaw_patrol_motor_PIDspeed 	 	= {0};
PID_TypeDef yaw_patrol_motor_PIDpos 		= {0};
PID_TypeDef pitch_patrol_motor_PIDspeed  	= {0};
PID_TypeDef pitch_patrol_motor_PIDpos 	 	= {0};
/***********閿熸枻鎷烽敓鏂ゆ嫹浣块敓鏂ゆ嫹*******************/
PID_TypeDef pitch_auto_aim_PIDspeed  	= {0};
PID_TypeDef pitch_auto_aim_PIDpos 	 	= {0};
PID_TypeDef yaw_auto_aim_PIDspeed 	 	= {0};
PID_TypeDef yaw_auto_aim_PIDpos 		= {0};

PID_TypeDef pitch_antitop_aim_PIDspeed 	= {0};
PID_TypeDef pitch_antitop_aim_PIDpos 	= {0};
PID_TypeDef yaw_antitop_aim_PIDspeed 	= {0};
PID_TypeDef yaw_antitop_aim_PIDpos 		= {0};

PID_TypeDef pitch_aoutpost_aim_PIDspeed	={0};
PID_TypeDef pitch_aoutpost_aim_PIDpos	={0};
PID_TypeDef yaw_aoutpost_aim_PIDspeed	={0};
PID_TypeDef yaw_aoutpost_aim_PIDpos		={0};

/* 閿熻妭璇ф嫹閿熸枻鎷烽敓鏂ゆ嫹 ------------------------------------------------------------------*/

float InverseLaplaceTransform_Discretization(float input,float output,float time);
float forward_feedback(float input,float* last_input,float time,float forward_gain);

float  GetPitchLimit_down(float downangle_limit,uint16_t angle_offset);
float  GetPitchLimit_up(float upangle_limit,uint16_t angle_offset);


//===================================================================================================================//
/****************************************************controller******************************************************/
//===================================================================================================================//
/**
  * @brief				PID閿熸枻鎷峰閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹6020閿熸枻鎷�
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
#ifdef GIMBAL_BOARD_UP
void Motor6020_PID_Init(void)
{
		/**********************************閿熸枻鎷峰閿熸枻鎷�********************************************/
	PID_Init(&pitch_startPIDpos, 30,0,0.0,0.05f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);
	PID_Init(&pitch_startPIDspeed,16384,500,0.0,150,100.0,
	0,5, 2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);	
	
	/**********************************閬ラ敓鏂ゆ嫹閿熸枻鎷锋ā寮忛敓閾扮鎷稰ID閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�********************************************/
	//yaw閿熸枻鎷�
	
	PID_Init(&yaw_rc_motor_PIDpos,18,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_rc_motor_PIDspeed,16384,0,0.0,2500,0,
	0,10,2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
	


	PID_Init(&pitch_rc_motor_PIDpos, 30,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_rc_motor_PIDspeed,16384,2000,0.0,5000,550.0,
	0,1.2, 0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
	
	/**********************************宸￠敓鏂ゆ嫹寮忛敓閾扮鎷稰ID閿熸枻鎷烽敓鏂ゆ嫹******************************************/
	//yaw閿熸枻鎷�
	PID_Init(&yaw_patrol_motor_PIDpos,18,0,0.0,15.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_patrol_motor_PIDspeed,16384,0,0.0,2000,0.0,
	0,10, 2, 0.5,0.5, Integral_Limit|Trapezoid_Intergral);
	
//	//pitch閿熸枻鎷�
	PID_Init(&pitch_patrol_motor_PIDpos,30,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_patrol_motor_PIDspeed,16384,2000,0.0,5000,550.0,
	0,1.2, 0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral | ChangingIntegralRate);
	

	/******************************************AUTO_AIM閿熸枻鎷烽敓鏂ゆ嫹*************************************************/
	//yaw閿熸枻鎷�
	PID_Init(&yaw_auto_aim_PIDpos,18,0,0.00,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_auto_aim_PIDspeed,16384,250,0.0,4500,100.0,
	0.0,0.8,0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral | ChangingIntegralRate);


	PID_Init(&pitch_auto_aim_PIDpos,30,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_auto_aim_PIDspeed,16384,2000,0.0,5200.0f,1000.0f,
	0,1.2, 0.3, 0.5,0.5, Integral_Limit |Trapezoid_Intergral |ChangingIntegralRate);
	 
	 
	
	PID_Init(&yaw_antitop_aim_PIDpos,18,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_antitop_aim_PIDspeed,16384,200,0.0,5000,100.0,
	0.0,1.0, 0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);
		
	PID_Init(&pitch_antitop_aim_PIDpos,30,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_antitop_aim_PIDspeed,16384,2000,0.0,5200,1200.0,
	0.0,1.2, 0.42, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);


		
	PID_Init(&yaw_aoutpost_aim_PIDpos,18,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_aoutpost_aim_PIDspeed,16384,200,0.0,5000,100.0,
	0.0,1.2, 0.4, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);
		
	PID_Init(&pitch_aoutpost_aim_PIDpos,30,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_aoutpost_aim_PIDspeed,16384,1200,0.0,5200,500.0,
	0.0,1.2, 0.3, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);

}
#endif 

#ifdef GIMBAL_BOARD_DOWN
	void Motor6020_PID_Init(void)
{
		/**********************************閿熸枻鎷峰閿熸枻鎷�********************************************/
	PID_Init(&pitch_startPIDpos, 30,0,0.0,10.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_startPIDspeed,16384,500,0.0,1600,100.0,
	0,5, 2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);	
	
	/**********************************閬ラ敓鏂ゆ嫹閿熸枻鎷锋ā寮忛敓閾扮鎷稰ID閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�********************************************/
	//yaw閿熸枻鎷�
	
	PID_Init(&yaw_rc_motor_PIDpos,20,0,0.0,15.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_rc_motor_PIDspeed,16384,0,0.0,3500,0,
	0,10,2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
	


	PID_Init(&pitch_rc_motor_PIDpos, 30,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_rc_motor_PIDspeed,16384,4000,0.0,3500,500.0,
	0,5, 2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
	
	/**********************************宸￠敓鏂ゆ嫹寮忛敓閾扮鎷稰ID閿熸枻鎷烽敓鏂ゆ嫹******************************************/
	//yaw閿熸枻鎷�
	PID_Init(&yaw_patrol_motor_PIDpos,20,0,0.0,18.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_patrol_motor_PIDspeed,16384,0,0.0,2500,0.0,
	0,10, 2, 0.5,0.5, Integral_Limit|Trapezoid_Intergral);
	
//	//pitch閿熸枻鎷�
	PID_Init(&pitch_patrol_motor_PIDpos,35,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_patrol_motor_PIDspeed,16384,4000,0.0,3500,20.0,
	0,1000, 60, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
	


	/******************************************AUTO_AIM閿熸枻鎷烽敓鏂ゆ嫹*************************************************/
	PID_Init(&yaw_auto_aim_PIDpos,20,0,0.0,20.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_auto_aim_PIDspeed,16384,200,0.0,4500,200.0,
	0.0,0.8,0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral | ChangingIntegralRate);

	PID_Init(&pitch_auto_aim_PIDpos,30,0.0f,0.0,18.0f,0.0f,
	0.0f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_auto_aim_PIDspeed,16384,1000,0.0,4200.0f,1200.0f,
	0,1.2, 0.35, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);
	 
	 


	PID_Init(&yaw_antitop_aim_PIDpos,20,0,0.0,20.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&yaw_antitop_aim_PIDspeed,16384,400,0.0,5200,500.0,
	0.0,1.0, 0.4, 0.5,0.5, Integral_Limit |Trapezoid_Intergral |ChangingIntegralRate);
		
	PID_Init(&pitch_antitop_aim_PIDpos,30,0,0.0,12.0f,0.0f,
	0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	PID_Init(&pitch_antitop_aim_PIDspeed,16384,400,0.0,4500.0,100.0,
	0.0,0.8, 0.2, 0.5,0.5, Integral_Limit |Trapezoid_Intergral|ChangingIntegralRate);
		

	// PID_Init(&yaw_aoutpost_aim_PIDpos,20,0,0.0,18.0f,0.0f,
	// 0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	// PID_Init(&yaw_aoutpost_aim_PIDspeed,16384,0,0.0,3800,500.0,
	// 0.0,2.2, 0.8, 0.5,0.5, Integral_Limit |Trapezoid_Intergral);
		
	// PID_Init(&pitch_aoutpost_aim_PIDpos,30,0,0.0,20.0f,0.0f,
	// 0.00f,7,2,0.5,0.6,Derivative_On_Measurement |Integral_Limit |Trapezoid_Intergral);
	// PID_Init(&pitch_aoutpost_aim_PIDspeed,16384,2000,0.0,5200,500.0,
	// 0.0,2.2, 0.8, 0.5,0.5, Integral_Limit |Trapezoid_Intergral | ChangingIntegralRate);	 
	 
}
#endif
  

//===================================================================================================================//
/****************************************************gimbal_action***************************************************/
//===================================================================================================================//



/*閿熸枻鎷峰彴閿熺潾鍑ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹(閿熸枻鎷烽敓鑺傛唻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�)*/ 
//娉ㄩ敓鏂ゆ嫹閿熸枻鎷峰彴閿熺潾鍑ゆ嫹閿熶茎瑙掕鎷峰簲閿熸枻鎷烽敓鏂ゆ嫹灏忛敓鏂ゆ嫹涓€閿熸枻鎷烽敓妗旀枻鎷�
/*
    @閿熸枻鎷烽敓鏂ゆ嫹1閿熸枻鎷穟pangle_limit   閿熸枻鎷烽敓鐨嗚璁规嫹(閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷峰€�)    閿熸枻鎷烽敓瑙掕鎷烽敓鏂ゆ嫹浣嶅€�
		@閿熸枻鎷烽敓鏂ゆ嫹2閿熸枻鎷穌ownangle_limit 閿熸枻鎷烽敓鐨嗚璁规嫹(閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷峰€�)    閿熸枻鎷烽敓瑙掕鎷烽敓鏂ゆ嫹浣嶅€�
		@閿熸枻鎷烽敓鏂ゆ嫹3閿熸枻鎷穉ngle_offset    閿熸枻鎷烽敓鏂ゆ嫹閿熺粸纭锋嫹閿熸枻鎷峰祵閿燂拷(閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷峰€�)
    retvl:閿熷壙鐚存嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹鍙杙itch閿熸枻鎷烽敓鏂ゆ嫹浣嶉敓鏂ゆ嫹閿熸枻鎷锋閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓锟�
*/
float  GetPitchLimit_down(float downangle_limit,uint16_t angle_offset)
{
	float down_totalangle_limit=0;
//閿熷彨璁规嫹閿熸枻鎷烽敓鏂ゆ嫹
	
	if(angle_offset<4096)
	{	
	 if(downangle_limit -angle_offset > 4096)
	  down_totalangle_limit=(downangle_limit -angle_offset -8192)*coder2radian;
	 else
		down_totalangle_limit=(downangle_limit -angle_offset)*coder2radian; 
  }
	else if(angle_offset>4096)
	{
	 down_totalangle_limit=(downangle_limit -angle_offset)*coder2radian;
  } 
	 return down_totalangle_limit;
}

float GetPitchLimit_up(float upangle_limit,uint16_t angle_offset)
{
   float up_totalangle_limit=0;
//閿熷彨璁规嫹閿熸枻鎷烽敓鏂ゆ嫹
	if(angle_offset<4096)
	{
	 	up_totalangle_limit =(upangle_limit -angle_offset)*coder2radian;
  }
	else if(angle_offset>4096)
	{
	if(upangle_limit -angle_offset < -4096)
	  up_totalangle_limit=(upangle_limit -angle_offset + 8192)*coder2radian;
	else
	up_totalangle_limit=(upangle_limit -angle_offset)*coder2radian; 
  } 
 return 	up_totalangle_limit;
}

void LimitPitch_encoder(target_set_t *target_set,float upangle_limit,float downangle_limit,uint16_t angle_offset)
{	
	target_set->up_totalangle_encoderlimit   = GetPitchLimit_up(upangle_limit,angle_offset);
	target_set->down_totalangle_encoderlimit = GetPitchLimit_down(downangle_limit,angle_offset);
	
	if(target_set->pitch_targetangle_remote > target_set->up_totalangle_encoderlimit)
		target_set->pitch_targetangle_remote = target_set->up_totalangle_encoderlimit;
	
	else if(target_set->pitch_targetangle_remote < target_set->down_totalangle_encoderlimit)
		target_set->pitch_targetangle_remote = target_set->down_totalangle_encoderlimit;	
	
	if(target_set->pitch_targetangle_patrol < target_set->down_totalangle_encoderlimit)
		target_set->pitch_targetangle_patrol=target_set->down_totalangle_encoderlimit;
	
	else if(target_set->pitch_targetangle_patrol > target_set->up_totalangle_encoderlimit)
		target_set->pitch_targetangle_patrol=target_set->up_totalangle_encoderlimit;	
}



/*閿熸枻鎷峰彴閿熺潾鍑ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹(閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熻鍑ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�)*/   
/*娉ㄩ敓鏂ゆ嫹閿熸枻鎷峰彴閿熺潾鍑ゆ嫹閿熶茎瑙掕鎷峰簲閿熸枻鎷烽敓鏂ゆ嫹灏忛敓鏂ゆ嫹涓€閿熸枻鎷烽敓妗旀枻鎷�
  閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷锋按骞抽敓瑙掕鎷峰€间负0*/
/*
    @閿熸枻鎷烽敓鏂ゆ嫹1閿熸枻鎷穟pangle_limit       閿熸枻鎷烽敓瑙掕鎷烽敓鏂ゆ嫹浣嶅€�
		@閿熸枻鎷烽敓鏂ゆ嫹2閿熸枻鎷穌ownangle_limit     閿熸枻鎷烽敓瑙掕鎷烽敓鏂ゆ嫹浣嶅€�

    retvl:閿熷壙鐚存嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹鍙杙itch閿熸枻鎷烽敓鏂ゆ嫹浣嶉敓鏂ゆ嫹閿熸枻鎷锋閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓锟�
*/
void LimitPitch_gyro(target_set_t *target_set,float upangle_limit,float downangle_limit)
{
	
	if(target_set->pitch_targetangle_gyro < downangle_limit)
		target_set->pitch_targetangle_gyro=downangle_limit;
	
	else if(target_set->pitch_targetangle_gyro > upangle_limit)
		target_set->pitch_targetangle_gyro=upangle_limit;	
	

}



//===================================================================================================================//
/****************************************************vision_judge****************************************************/
//===================================================================================================================//
/**
  * @brief				閿熸帴鎾呮嫹閿熸枻鎷峰抚閿熷彨鏂尨鎷烽敓鏂ゆ嫹
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
void vision_function_sentry(void)
{
		//閿熷彨璁规嫹璇嗛敓鏂ゆ嫹閿熸枻鎷风姸鎬�
		Identification_status = vision_judgment_sentry(last_minipc_rx.state_flag,minipc_rx.state_flag);
}


/**
  * @brief				閿熸帴鎾呮嫹閿熷彨鏂尨鎷烽敓鏂ゆ嫹
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
uint8_t vision_judgment_sentry (uint8_t last_state_flag,uint8_t state_flag)
{
	if(last_state_flag == Lose_Traget && state_flag == Lose_Traget)								//涓€鐩存病閿熸枻鎷疯瘑閿熸枻鎷风洰閿熸枻鎷� 
		{
		return Losetraget2Losetraget;
		}
			
	else if(last_state_flag == Lose_Traget && state_flag == Find_Traget)				//閿熸帴璁规嫹澶辩洰閿熺枼鍒拌瘑閿熸枻鎷风洰閿熸枻鎷�//
		{
//		gimbal_status.gimbal_mode = AUTO_AIM_MODE;
				
		return Losetraget2Findtraget;
		}
			
	else if(last_state_flag == Find_Traget && state_flag == Find_Traget)				//涓€鐩磋瘑閿熸枻鎷风洰閿熸枻鎷�
		{
//		gimbal_status.gimbal_mode = AUTO_AIM_MODE;
				
		return Findtraget2Findtraget;
		}
			
	else if(last_state_flag == Find_Traget && state_flag == Lose_Traget)				//閿熸枻鎷疯瘑閿熸枻鎷风洰閿熺枼鍒伴敓鏂ゆ嫹澶辩洰閿熸枻鎷� 閿熸枻鎷蜂竴閿熻娇璁规嫹澶遍敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹澶辩洰閿熸枻鎷�
		{
			return Findtraget2Losetraget;
		}
		return Losetraget2Losetraget;
}





//===================================================================================================================//
/****************************************************mode_change*****************************************************/
//===================================================================================================================//
/**
  * @brief				閿熸枻鎷峰彴妯″紡杞敓鏂ゆ嫹 鐩敓鏂ゆ嫹鍊奸敓鏂ゆ嫹閿熸枻鎷�
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
void Convermode_set(Gimbal_Status_t *gimbal_status)
{
	GetGimbalConvermode(gimbal_status);
	
	//閿熸枻鎷烽敓鏂ゆ嫹閿熷彨浼欐嫹閿熸枻鎷烽敓閰碉綇鎷烽敓鏂ゆ嫹閿熷彨鎲嬫嫹瑕侀敓鏂ゆ嫹鍊奸敓渚ラ潻鎷峰€�
	switch(gimbal_status->gimbal_Convermode)
	{
	  case now2now:break;
		case now2remote:
		{
			//閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽劊鎱曞祵閿熼ズ锔兼嫹閿燂拷 
//			motor_param_get[YAW_MOTOR].round_cnt=0;		
			//閿熸枻鎷烽敓鏂ゆ嫹total 鍊�
//			motor_param_get[YAW_MOTOR].total_angle=motor_param_get[YAW_MOTOR].angle-motor_param_get[YAW_MOTOR].offset_angle;
			
			INS.Yaw.angle_round=0;
			//閿熸枻鎷烽敓鏂ゆ嫹total 鍊�			
			INS.Yaw.total_radangle=INS.Yaw.ins_angle*angle2radian;
			
			target_set.yaw_targetangle_remote				= INS.Yaw.total_radangle;
			target_set.pitch_targetangle_remote	 		= motor_param_get[PITCH_MOTOR].total_radangle;
			
					
			PIDCalc_Clear(&yaw_rc_motor_PIDspeed);
			PIDCalc_Clear(&yaw_rc_motor_PIDpos);
			PIDCalc_Clear(&pitch_rc_motor_PIDspeed);			
		  PIDCalc_Clear(&pitch_rc_motor_PIDpos);		
		}break;
		case now2patrol:
		{
//			motor_param_get[YAW_MOTOR].round_cnt=0;
//			//閿熸枻鎷烽敓鏂ゆ嫹total 鍊�
//			motor_param_get[YAW_MOTOR].total_angle=motor_param_get[YAW_MOTOR].angle-motor_param_get[YAW_MOTOR].offset_angle;
			
			INS.Yaw.angle_round=0;
			//閿熸枻鎷烽敓鏂ゆ嫹total 鍊�			
			INS.Yaw.total_radangle=INS.Yaw.ins_angle*angle2radian;
			
		  target_set.yaw_targetangle_patrol			= INS.Yaw.total_radangle;
			target_set.pitch_targetangle_patrol	 		= motor_param_get[PITCH_MOTOR].total_radangle;
				
			
			PIDCalc_Clear(&yaw_patrol_motor_PIDspeed);
			PIDCalc_Clear(&yaw_patrol_motor_PIDpos);
			PIDCalc_Clear(&pitch_patrol_motor_PIDspeed);			
		  PIDCalc_Clear(&pitch_patrol_motor_PIDpos);		
		}break;
		case now2autoaim:
		{
			INS.Yaw.angle_round=0;
			//閿熸枻鎷烽敓鏂ゆ嫹total 鍊�			
			INS.Yaw.total_radangle=INS.Yaw.ins_angle*angle2radian;
			
		  target_set.yaw_targetangle_gyro			= INS.Yaw.total_radangle;
			target_set.pitch_targetangle_gyro	 		= INS.Pitch.total_radangle;
			
			PIDCalc_Clear(&pitch_auto_aim_PIDspeed);
			PIDCalc_Clear(&pitch_auto_aim_PIDpos);
			PIDCalc_Clear(&yaw_auto_aim_PIDspeed);			
		  PIDCalc_Clear(&yaw_auto_aim_PIDpos);		
	
			}break;
		
		default:break;
	}
}


/**
  * @brief				閿熸枻鎷峰彇閿熸枻鎷峰彴妯″紡閿熸枻鎷烽敓鏂ゆ嫹妯″紡
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
void GetGimbalConvermode(Gimbal_Status_t *gimbal_status)
{
	//閿熸枻鎷峰閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷疯阿閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓渚ワ吉锟�
	
	//閿熸枻鎷峰墠妯″紡涓洪仴閿熸枻鎷锋ā寮�
	if(gimbal_status->gimbal_mode == REMOTE_MODE)
	{
		switch(gimbal_status->last_gimbal_mode)
		{
			case AUTO_AIM_MODE:
			case PATROL_MODE:
			{
				gimbal_status->gimbal_Convermode = now2remote;
			}break;
			case REMOTE_MODE:
			{
				gimbal_status->gimbal_Convermode=remote2remote;
			}break;
			default:break;
		}
	}
		//閿熸枻鎷峰墠妯″紡涓哄贰閿熸枻鎷锋ā寮�
	else if(gimbal_status->gimbal_mode == PATROL_MODE)
		{
			switch(gimbal_status->last_gimbal_mode)
			{
				case REMOTE_MODE:
				case AUTO_AIM_MODE:
				{
					gimbal_status->gimbal_Convermode = now2patrol;
				}break;
				case PATROL_MODE:
				{
					gimbal_status->gimbal_Convermode=patrol2patrol;
				}break;
				default:break;
			}
		}
		//閿熸枻鎷峰墠妯″紡涓洪敓鏂ゆ嫹閿熸枻鎷锋ā寮�			
	else if(gimbal_status->gimbal_mode == AUTO_AIM_MODE)
		{
			switch(gimbal_status->last_gimbal_mode)
			{
				case REMOTE_MODE:
				case PATROL_MODE:
				{
					gimbal_status->gimbal_Convermode = now2autoaim;
				}break;
				case AUTO_AIM_MODE:
				{
					gimbal_status->gimbal_Convermode=autoaim2autoaim;
				}break;
				default:break;
			}
		}
}


//===================================================================================================================//
/****************************************************forward_feedback************************************************/
//===================================================================================================================//
/**
  * @brief				閿熸枻鎷烽敓鐨嗕紮鎷烽敓鑺傜尨鎷烽敓鏂ゆ嫹
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
float InverseLaplaceTransform_Discretization(float input,float output,float time)
{
	float dt = 0.001;
	//T閿熻鐧告嫹閿熺殕绛规嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鐨嗙鎷烽敓鏂ゆ嫹瓒婇敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹瓒婇敓鏂ゆ嫹
	float T = 0.02;
	//閿熸枻鎷烽敓鏂ゆ嫹鏃堕敓鏂ゆ嫹閿熸枻鎷�
	dt = time;
	
	output = (input*dt + (T - dt)*output)/T;
	
	return output;
}


/**
  * @brief				鍓嶉敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓濮愬嚱閿熸枻鎷�
  * @param[in]		
	* @param[out]		
  * @retval				none
*/
float forward_feedback(float input,float* last_input,float time,float forward_gain)
{
	float output;
	
	static float dt = 0.001;
	
	if(*last_input == 0)
	{
		*last_input = input;
		return 0;
	}
	
	//閿熸枻鎷烽敓鏂ゆ嫹鏃堕敓鏂ゆ嫹閿熸枻鎷�
	dt = time;
	
	output = (input - *last_input)/dt;

	*last_input = input;
	
	return output*forward_gain;
}



float PID_YAW_CAL_forward(PID_TypeDef* PID_POS,PID_TypeDef* PID_SPEED,float measure_angle,float measure_speed,float expect_yaw,float dt,float T,float Gain)
{			
	
	float	current_output;
	
	static float forward_current,all_value;
	static float expect_filter,expect_filter_last;
	
	//0.1涓洪敓鏂ゆ嫹閫氶敓鍓胯鎷烽敓鏂ゆ嫹閿熺殕绛规嫹閿熸枻鎷穞,T涓洪敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�
	expect_filter=(dt*expect_filter_last+T*expect_yaw)/(dt+T);

	forward_current =forward_feedback(expect_filter,&expect_filter_last,T,Gain);

	//閿熻搴︿紮鎷�
	PID_Calculate(PID_POS , measure_angle ,expect_yaw);
	//鍓嶉敓鏂ゆ嫹閿熸枻鎷烽敓锟�
	all_value = forward_current + PID_POS->Output;	
	//閿熷姭搴︿紮鎷�
	PID_Calculate(PID_SPEED , measure_speed , all_value) ;

	current_output = PID_SPEED->Output;		
    expect_filter_last=expect_filter;			 
	return current_output ;
}


float  PID_PITCH_CAL_forward(PID_TypeDef* PID_POS,PID_TypeDef* PID_SPEED,float measure_angle,float measure_speed,float expect_pitch,float dt,float T,float Gain)
{
	float   current_output;
	static float forward_current,all_value=0;
	static float expect_filter,expect_filter_last;
	
	//0.05涓洪敓鏂ゆ嫹閫氶敓鍓胯鎷烽敓鏂ゆ嫹閿熺殕绛规嫹閿熸枻鎷穞,T涓洪敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�
	expect_filter=(dt*expect_filter_last+T*expect_pitch)/(dt+T);
	
	//閿熷姭璁规嫹鍓嶉敓鏂ゆ嫹
	forward_current =forward_feedback(expect_pitch,&expect_filter,T,Gain);
	//閿熻搴︿紮鎷�
	PID_Calculate(PID_POS , measure_angle , expect_pitch);

	all_value = forward_current + PID_POS->Output; //forward_current

	PID_Calculate(PID_SPEED , measure_speed ,all_value ) ;
	
	current_output = PID_SPEED->Output ; 
	expect_filter_last=expect_filter;

	return current_output;
}



